clc;clear;
close all
warning off all

%% 初始化模型
my_bot()
testcarmain

%% 消息类型常量
ClientAccept = uint32(0);
PingServer = uint32(1);
CreateAgent = uint32(2);
POST_WorldState = uint32(3);
POST_AgentState = uint32(4);
POST_PathState = uint32(5);
POST_MapState = uint32(6);
GET_WorldState = uint32(7);
GET_AgentState = uint32(8);
GET_PathState = uint32(9);
GET_MapState = uint32(10);

%% 设置连接参数，要连接的地址为43.142.137.93(即本地主机)，端口号为60000，作为客户机连接。
Client=tcpip('43.142.137.93',60000,'NetworkRole','client');
fprintf("init Client.BytesAvailable: %d\n", Client.BytesAvailable);

%% 建立连接，建立完成后进行下一步，否则报错
fopen(Client);%与一个服务器建立连接，直到建立完成返回，否则报错。
fprintf('成功建立连接');
%% 获取验证token
while true
    if Client.BytesAvailable
        fprintf("Client.BytesAvailable before token: %d\n", Client.BytesAvailable);
        token = uint32(fread(Client, 1, "uint32"));
        fprintf("token = %d\n", token);
        fprintf("Client.BytesAvailable after token: %d\n", Client.BytesAvailable);
        break;
    end
end

%% 发回服务端
fwrite(Client, token, "uint32");
pause(1);

%% 接收验证通过后的 ServerAccept 消息
 % 应该为 8 个uint8(0)
len = Client.BytesAvailable;
accept = char(zeros(1,len));
accept = fread(Client, [1,len], "uint8")

%% 发送 CreateAgent 消息
fwrite(Client, uint32(CreateAgent), "uint32");
fwrite(Client, uint32(0), "uint32");

%% 接收 POST_AgentState 消息进行初始化
% 确定消息类型
while true
    if Client.BytesAvailable
        fprintf("Client.BytesAvailable before type: %d\n", Client.BytesAvailable);
        type = uint32(fread(Client, 1, "uint32"));
        fprintf("Client.BytesAvailable after type: %d\n\n", Client.BytesAvailable);
        break;
    end
end


% 如果是 POST_AgentState 消息，收 body 大小
if POST_AgentState == type
    while true
        if Client.BytesAvailable
            fprintf("Client.BytesAvailable before len: %d\n", Client.BytesAvailable);
            len = fread(Client, 1, "uint32");
            fprintf("len =  %d\n", len);
            fprintf("Client.BytesAvailable before len: %d\n\n", Client.BytesAvailable);
            break;
        end
    end
end

% 接收 body 部分
json = char(zeros(1,len));
while true
    if Client.BytesAvailable
        fprintf("Client.BytesAvailable before json: %d\n", Client.BytesAvailable);
        % 别管效率高的提示，这样反正不会错，改了就不知道了
        json = char(fread(Client, [1 len], "uint8"));
        fprintf("json = \n'%s'\n", json);
        fprintf("Client.BytesAvailable after json: %d\n\n", Client.BytesAvailable);
        break;
    end
end

%% 初始化 agent
init = jsondecode(json);
agent = init.agent;
% agent.position

%% 发送 GET_PathState消息
fwrite(Client, uint32(GET_PathState), "uint32");
fwrite(Client,uint32(length(json)), "uint32");
fwrite(Client, uint8(char(json)), "uint8");

%% 接收 POST_PathState 消息进行初始化
% 确定消息类型
while true
    if Client.BytesAvailable
        fprintf("Client.BytesAvailable before type: %d\n", Client.BytesAvailable);
        type = uint32(fread(Client, 1, "uint32"));
        fprintf("type = %d\n", type);
        fprintf("Client.BytesAvailable after type: %d\n\n", Client.BytesAvailable);
        break;
    end
end


% 如果是 POST_PathState 消息，收 body 大小
if POST_PathState == type
    while true
        if Client.BytesAvailable
            fprintf("Client.BytesAvailable before len: %d\n", Client.BytesAvailable);
            len = fread(Client, 1, "uint32");
            fprintf("len = %d\n", len);
            fprintf("Client.BytesAvailable before len: %d\n\n", Client.BytesAvailable);
            break;
        end
    end
end
pause(1);

% 接收 body 部分
% 数据量大时会分为 512 byte 的包
json = char(zeros(1,len));
recv_len = uint32(0);
while recv_len < len
    av = Client.BytesAvailable;
    fprintf("Client.BytesAvailable before json: %d %d\n", Client.BytesAvailable, av);
    json(1, recv_len + 1:recv_len + av) = char(fread(Client, [1 av], "uint8"));
    % fprintf("json =  \n'%s'\n", json);
    fprintf("Client.BytesAvailable after json: %d\n\n", Client.BytesAvailable);
    recv_len = recv_len + av;
end

%% 将收到的 path 存到 desire_Pos 数组里
path = jsondecode(json);
path = path.path
desire_Pos = path.nodes'
size(desire_Pos)

%% 运动学部分
K = 0.5;
l = 0.2;
delta_T = 0.5;
position = [agent.position.x, agent.position.y, agent.direction];  % x, y, Theta
T = linspace(0,20,20);

f3 = figure;
xlabel('x (m)')
ylabel('y (m)')
grid on

%小车位姿
x = position(1);
y = position(2);

this.robot_handle{1} = patch(...
'Vertices', transformed(:, 1:2), ...
'Faces', data.faces, ...
'FaceColor', 'flat', ...
'FaceVertexCData', data.colors, ...
'EdgeColor','none');


data = my_bot;
this.robot_body = data.vertices;
theta = position(3) - pi/2;
rotation_matrix = [
    cos(theta) -sin(theta) x;
    sin(theta)  cos(theta) y;
    0 0 1];

%-----------------------------绘制图像
h = animatedline('color','r','LineStyle','--');
h_car = animatedline('color','b');
h_car_model = animatedline('Marker','o','MaximumNumPoints',1);     %只显示1个新的点
transformed = this.robot_body * rotation_matrix';

for k = 1 : path.length
    addpoints(h_car,position(1),position(2));
    addpoints(h_car_model,position(1),position(2));
    axis([0 64 0 32]) % Map size

    error_x = position(1)-desire_Pos(k).x;
    error_y = position(2)-desire_Pos(k).y;
    predict_x = -K*error_x;
    predict_y = -K*error_y;
    A_mat = [cos(position(3)) -l*sin(position(3));sin(position(3)) l*cos(position(3))];
    U_mat = [predict_x, predict_y]';
    v_w_mat = A_mat\U_mat;
    position(1) = position(1)+v_w_mat(1)*cos(position(3))-v_w_mat(2)*l*sin(position(3));
    position(2) = position(2)+v_w_mat(1)*sin(position(3))+v_w_mat(2)*l*cos(position(3));  
    position(3) = position(3)+v_w_mat(2)*delta_T;
   
    x = position(1);
    y = position(2);
    theta = position(3) - pi/2;
    rotation_matrix = [
        cos(theta) -sin(theta) x;
        sin(theta)  cos(theta) y;
        0 0 1];
    transformed = this.robot_body * rotation_matrix';
    set(this.robot_handle{1}, 'Vertices', transformed(:, 1:2));
    addpoints(h,desire_Pos(k).x,desire_Pos(k).y);
    
    frame=getframe(gcf);
    image = frame2im(frame); 
    [imind,cm] = rgb2ind(image,256);

    %% 构造 Agent::State
    agent.id;
    agent.shape;
    agent.radius=0.5;
    % 速度是标量
    agent.velocity = norm(predict_x:predict_y);
    % 方向是与x轴的夹角(rad)
    agent.direction = theta + 1/2 * pi;
    % 角速度是标量(rad)
    agent.angularVelocity = norm(v_w_mat);
    % 加速度是标量
    agent.acceleration = norm(U_mat);
    % 位置是两个标量
    agent.position.x = x;
    agent.position.y = y;
    
    obj.agent = agent;
    cardata = uint8(char(jsonencode(obj)));
    fprintf("data len = %d\n", length(cardata));
    % 查看变量信息的函数
    fprintf("data = \n'%s'\n", char(cardata));
    
    %% 发送字符串数据，必须这么写，否则服务端解析不了
    % fwrite(Client, uint8(char(data)), "uint8");
    %% 发送 POST_AgentState 消息
    fwrite(Client, uint32(POST_AgentState), "uint32");
    fwrite(Client,uint32(length(cardata)), "uint32");
    fwrite(Client, uint8(char(cardata)), "uint8");

    pause(1);
end

%% 关闭客户端
fclose(Client);


